/*
 * CaminoMinimo.cpp
 *
 *  Created on: 07/04/2013
 *      Author: Juliansci
 */

#include "CaminoMinimo.h"
#include <stdlib.h>
#include <math.h>

NodoCamino::NodoCamino(int dist, int heuristica, Casilla* prev, Casilla* cas) {
	distancia = dist;
	previo = prev;
	casilla = cas;
	costoHeuristica = heuristica;
}
int NodoCamino::getDistancia() {
	return distancia;
}
int NodoCamino::getCostoHeuristica(){
	return costoHeuristica;
}
Casilla* NodoCamino::getCasilla() {
	return casilla;
}
Casilla* NodoCamino::getPrevio() {
	return previo;
}
void NodoCamino::setDistancia(int dist) {
	distancia = dist;
}
void NodoCamino::setCostoHeuristica(int costo){
	costoHeuristica = costo;
}
void NodoCamino::setPrevio(Casilla* prev) {
	previo = prev;
}
void NodoCamino::setCasilla(Casilla* cas) {
	casilla = cas;
}

CaminoMinimo::CaminoMinimo(Escenario* escenario) {
	this->escenario = escenario;
	for (int i = 0; i < escenario->getAlto(); i++){
		vector<NodoCamino*> fila(escenario->getAncho());
		this->nodos.push_back(fila);
	}
}
NodoCamino* CaminoMinimo::obtenerNodoMinimo(list<NodoCamino*> noVisitados) {
	list<NodoCamino*>::iterator itNoVisitados;
	list<NodoCamino*> candidatos;
	itNoVisitados = noVisitados.begin();
	NodoCamino* nodoMinimo = (*itNoVisitados);
	int minimo = (*itNoVisitados)->getCostoHeuristica();
	for (itNoVisitados = noVisitados.begin();
			itNoVisitados != noVisitados.end(); itNoVisitados++) {

		if ((*itNoVisitados)->getCostoHeuristica() <= minimo) {
			nodoMinimo = (*itNoVisitados);
			minimo = nodoMinimo->getCostoHeuristica();
			candidatos.push_back(nodoMinimo);
		}
	}
	return nodoMinimo;
}

bool CaminoMinimo::sonVecinos(NodoCamino* nodo1, NodoCamino* nodo2) {
	int x1 = nodo1->getCasilla()->getX();
	int y1 = nodo1->getCasilla()->getY();
	int x2 = nodo2->getCasilla()->getX();
	int y2 = nodo2->getCasilla()->getY();
	int dX = x1 - x2;
	int dY = y1 - y2;
	if ((abs(dX) <= 1) && (abs(dY) <= 1))
		return true;
	return false;
}

bool CaminoMinimo::esPosicionValida(int x, int y){
	if(x >= 0 && y >= 0 && x < this->escenario->getAncho() && y < this->escenario->getAlto()){
		return true;
	}
	return false;
}

list<NodoCamino*> CaminoMinimo::obtenerVecinosTransitables(NodoCamino* origen){
	Casilla* casillaOrigen = origen->getCasilla();
	int x = casillaOrigen->getX();
	int y = casillaOrigen->getY();

	list<NodoCamino*> vecinos;

	if(esPosicionValida(x-1,y+1) && escenario->getCasilla(x-1,y+1)->esTransitable()){
		vecinos.push_back(nodos[x-1][y+1]);
	}
	if(esPosicionValida(x,y+1) && escenario->getCasilla(x,y+1)->esTransitable()){
		vecinos.push_back(nodos[x][y+1]);
	}
	if(esPosicionValida(x+1,y+1) && escenario->getCasilla(x+1,y+1)->esTransitable()){
		vecinos.push_back(nodos[x+1][y+1]);
	}
	if(esPosicionValida(x-1,y) && escenario->getCasilla(x-1,y)->esTransitable()){
		vecinos.push_back(nodos[x-1][y]);
	}
	if(esPosicionValida(x+1,y) && escenario->getCasilla(x+1,y)->esTransitable()){
		vecinos.push_back(nodos[x+1][y]);
	}
	if(esPosicionValida(x-1,y-1) && escenario->getCasilla(x-1,y-1)->esTransitable()){
		vecinos.push_back(nodos[x-1][y-1]);
	}
	if(esPosicionValida(x,y-1) && escenario->getCasilla(x,y-1)->esTransitable()){
		vecinos.push_back(nodos[x][y-1]);
	}
	if(esPosicionValida(x+1,y-1) && escenario->getCasilla(x+1,y-1)->esTransitable()){
		vecinos.push_back(nodos[x+1][y-1]);
	}
	return vecinos;
}

list<NodoCamino*> CaminoMinimo::obtenerVecinos(NodoCamino* origen){
	Casilla* casillaOrigen = origen->getCasilla();
	int x = casillaOrigen->getX();
	int y = casillaOrigen->getY();

	list<NodoCamino*> vecinos;

	if(esPosicionValida(x-1,y+1)){
		vecinos.push_back(nodos[x-1][y+1]);
	}
	if(esPosicionValida(x,y+1)){
		vecinos.push_back(nodos[x][y+1]);
	}
	if(esPosicionValida(x+1,y+1)){
		vecinos.push_back(nodos[x+1][y+1]);
	}
	if(esPosicionValida(x-1,y)){
		vecinos.push_back(nodos[x-1][y]);
	}
	if(esPosicionValida(x+1,y)){
		vecinos.push_back(nodos[x+1][y]);
	}
	if(esPosicionValida(x-1,y-1)){
		vecinos.push_back(nodos[x-1][y-1]);
	}
	if(esPosicionValida(x,y-1)){
		vecinos.push_back(nodos[x][y-1]);
	}
	if(esPosicionValida(x+1,y-1)){
		vecinos.push_back(nodos[x+1][y-1]);
	}
	return vecinos;
}


bool CaminoMinimo::perteneceA(NodoCamino* buscado, list<NodoCamino*> donde){
	list<NodoCamino*>::iterator it = donde.begin();
	while(it != donde.end()){
		if(*it == buscado){
			return true;
		}
		it++;
	}
	return false;
}

int CaminoMinimo::calcularHeuristica(NodoCamino* origen, Casilla* final){
	Casilla* origenCasilla = origen->getCasilla();
	int dx = abs(origenCasilla->getX() - final->getX());
	int dy = abs(origenCasilla->getY() - final->getY());
	return  round((dx + dy) + sqrt(2) * min(dx, dy));
}

Casilla* CaminoMinimo::reemplazarFinal(NodoCamino* finalOriginal){
	Casilla* nuevoFinal = NULL;
	list<NodoCamino*> vecinos;
	vecinos.push_back(finalOriginal);
	list<NodoCamino*> nuevoVecinos;
	for(int i = 0; i < 3; i++){
		for(list<NodoCamino*>::iterator it = vecinos.begin(); it != vecinos.end(); it++){
			nuevoVecinos = obtenerVecinos(*it);
			for(list<NodoCamino*>::iterator it2 = nuevoVecinos.begin(); it2 != nuevoVecinos.end(); it2++){
				if((*it2)->getCasilla()->esTransitable()){
					return (*it2)->getCasilla();
				}
			}
		}
		vecinos = nuevoVecinos;
	}
	return nuevoFinal;
}

list<Casilla*> CaminoMinimo::getCaminoMinimo(Casilla* inicial, Casilla* final) {
	int ancho = escenario->getAncho();
	int alto = escenario->getAlto();
	Casilla* casillaFinal = final;

	list<Casilla*> caminoMinimo;
	list<NodoCamino*> abierto;
	list<NodoCamino*> cerrado;

	for (int i = 0; i < ancho; i++) {
		for (int j = 0; j < alto; j++) {
			NodoCamino* nodoVacio = new NodoCamino(INF, INF, NULL, escenario->getCasilla(i,j));
			nodos[i][j] = nodoVacio;
		}
	}

	if(!casillaFinal->esTransitable()){
		if(sonVecinos(nodos[inicial->getX()][inicial->getY()], nodos[final->getX()][final->getY()])){
			return caminoMinimo;
		}
		else{
			casillaFinal = reemplazarFinal(nodos[final->getX()][final->getY()]);
			if(casillaFinal == NULL){
						return caminoMinimo;
			}
		}
	}

	NodoCamino* nodoNoVisitado = nodos[inicial->getX()][inicial->getY()];
	abierto.push_back(nodoNoVisitado);

	NodoCamino* nodoMinimo = obtenerNodoMinimo(abierto);
	Casilla* actual = nodoMinimo->getCasilla();

	nodos[actual->getX()][actual->getY()]->setDistancia(0); //Distancia

	while (actual->getX()!=casillaFinal->getX() || actual->getY()!=casillaFinal->getY()) {

		abierto.remove(nodoMinimo);
		cerrado.push_back(nodoMinimo);
		list<NodoCamino*> vecinos = obtenerVecinosTransitables(nodoMinimo);

		for(list<NodoCamino*>::iterator it = vecinos.begin(); it != vecinos.end(); it++){
			NodoCamino* vecino = *it;
			int costo = nodoMinimo->getDistancia() + 1;
			if(perteneceA(vecino,abierto)){
				if(costo < vecino->getDistancia()){
					abierto.remove(vecino);
				}
			}
			else if(perteneceA(vecino,cerrado)){
				if(costo < vecino->getDistancia()){
					cerrado.remove(vecino);
				}
			}
			else {
				vecino->setDistancia(costo);
				abierto.push_back(vecino);
				vecino->setCostoHeuristica(vecino->getDistancia() + calcularHeuristica(vecino,casillaFinal));
				vecino->setPrevio(nodoMinimo->getCasilla());
			}
		}

		if(!abierto.empty()){
			nodoMinimo = obtenerNodoMinimo(abierto);
			actual = nodoMinimo->getCasilla();
		}
		else{
			nodoMinimo = nodos[inicial->getX()][inicial->getY()];
			break;
		}
	}


	//Obtengo el camino minimo
	cout<<"Cam min"<<endl;
	NodoCamino* nodoCaminoActual = nodoMinimo;
	Casilla* nodoCaminoActualCasilla = NULL;
	while (nodoCaminoActual->getPrevio() != NULL) {
		nodoCaminoActualCasilla = nodoCaminoActual->getCasilla();
		caminoMinimo.push_front(nodoCaminoActualCasilla);
		nodoCaminoActual = nodos[nodoCaminoActual->getPrevio()->getX()][nodoCaminoActual->getPrevio()->getY()];
	}

	//Libero recursos

	for (int i = 0; i < ancho; i++) {
		for (int j = 0; j < alto; j++) {
			delete nodos[i][j];
		}
	}

	return caminoMinimo;

}
